#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/CommandLong.h>
#include <mavros_msgs/WaypointPull.h>
#include <mavros_msgs/WaypointPush.h>
#include <mavros_msgs/WaypointSetCurrent.h>
#include <mavros_msgs/GlobalPositionTarget.h>
#include <geographic_msgs/GeoPoseStamped.h>
#include <mavros_msgs/State.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <cmath>
#include <math.h>
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/PositionTarget.h>
#include <unistd.h>
#include <vector>
#include <ros/duration.h>
#include <iostream>
#include <string>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/Point.h>
#include <cmath>
#include <Eigen/Geometry>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
//#include <ar_track_alvar_msgs/AlvarMarker.h>
#include <std_msgs/Bool.h>
#include <ros/time.h> 
#include <ros/duration.h>
#include <sensor_msgs/LaserScan.h>
#define ANGLE_TO_RAD(x)    ((x) * M_PI / 180.0)     
#define RADIUS 6371000.0f // 地球平均半径（单位：米）

float degreesToRadians(float degrees) {
    return degrees * M_PI / 180.0f;
}
mavros_msgs::State current_state_g;
nav_msgs::Odometry current_pose_g;
geometry_msgs::Pose correction_vector_g;
geometry_msgs::Point local_offset_pose_g;
geometry_msgs::PoseStamped waypoint_g;
sensor_msgs::NavSatFix GPSCoordinate;
double REFERENCE_LATITUDE=0;
double REFERENCE_LONGITUDE=0;
using namespace Eigen;
using namespace std;
float current_heading_g;
float local_offset_g;//heading
float correction_heading_g = 0;
float local_desired_heading_g; 
float currentlat;
float currentlon;
float desire_yaw_;//期望的飞机相对降落板的偏航角
float desire_yawVel_;
float desire_pose_x,desire_pose_y,desire_pose_z;
bool detect_state=false;
typedef struct
{
	float vel_x;
	float vel_y;
	float vel_z;
} S_SETPOINT_VEL;
typedef struct
{
	float difference;      //比例项
	float differential;    //微分项
	float tempDiffer;			 //上一时刻的比例项
	float intergral;       //积分项
} S_PID_ITEM;
typedef struct
{
	float p;       //比例项系数
	float d;       //微分项系数.
	float i;       //积分项系数
} S_PID;

Eigen::Vector4d desire_vel_;
Eigen::Vector3d desire_xyVel_;
Eigen::Vector3d desire_pose_;//期望的飞机相对降落板的位置
S_PID s_PidXY,s_PidZ,s_PidYaw;
S_PID_ITEM s_PidItemX;
S_PID_ITEM s_PidItemY;
S_PID_ITEM s_PidItemZ;
S_PID_ITEM s_PidItemYaw;

ros::Publisher local_pos_pub;
ros::Publisher global_lla_pos_pub;
ros::Publisher global_lla_pos_pub_raw;
ros::Subscriber currentPos;
ros::Subscriber state_sub;
ros::Subscriber gps_sub;
ros::ServiceClient arming_client;
ros::ServiceClient land_client;
ros::ServiceClient set_mode_client;
ros::ServiceClient takeoff_client;
ros::ServiceClient command_client;
ros::ServiceClient auto_waypoint_pull_client;
ros::ServiceClient auto_waypoint_push_client;
ros::ServiceClient auto_waypoint_set_current_client;
/**
\ingroup control_functions
This structure is a convenient way to format waypoints
*/




struct gnc_api_waypoint{
	float x; ///< distance in x with respect to your reference frame
	float y; ///< distance in y with respect to your reference frame
	float z; ///< distance in z with respect to your reference frame
	float psi; ///< rotation about the third axis of your reference frame
};


class APMLanding {
public:
	ros::Publisher mavros_setpoint_pos_pub_;
	void send_body_velxyz_setpoint(const Eigen::Vector3d& vel_sp, float yaw_sp);
	Eigen::Vector4d LandingPidProcess(Eigen::Vector3d &currentPos,float currentYaw,Eigen::Vector3d &expectPos,float expectYaw)
	{
	  Eigen::Vector4d s_PidOut;
		/*X方向的pid控制*///位置式PID控制
		s_PidItemX.difference = expectPos[0] - currentPos[0];//e(k)
		
		s_PidItemX.intergral += s_PidItemX.difference;
		if(s_PidItemX.intergral >= 50)		
			s_PidItemX.intergral = 50;
		else if(s_PidItemX.intergral <= -50) 
			s_PidItemX.intergral = -50;
		s_PidItemX.differential =  s_PidItemX.difference  - s_PidItemX.tempDiffer;//e(k-1)
	  s_PidItemX.tempDiffer = s_PidItemX.difference;

		s_PidOut[0] = (s_PidXY.p*s_PidItemX.difference + s_PidXY.d*s_PidItemX.differential + s_PidXY.i*s_PidItemX.intergral);
		/*Y方向的pid控制*/
		s_PidItemY.difference = expectPos[1] - currentPos[1];
		s_PidItemY.intergral += s_PidItemY.difference;
		if(s_PidItemY.intergral >= 50)		
			s_PidItemY.intergral = 50;
		else if(s_PidItemY.intergral <= -50) 
			s_PidItemY.intergral = -50;
		s_PidItemY.differential =  s_PidItemY.difference  - s_PidItemY.tempDiffer;
	  s_PidItemY.tempDiffer = s_PidItemY.difference;
		s_PidOut[1] =(s_PidXY.p*s_PidItemY.difference + s_PidXY.d*s_PidItemY.differential + s_PidXY.i*s_PidItemY.intergral);
			
		/*Z方向的pid控制*/
		s_PidItemZ.difference = expectPos[2] - currentPos[2];
		s_PidItemZ.intergral += s_PidItemZ.difference;
		if(s_PidItemZ.intergral >= 100)		
			s_PidItemZ.intergral = 100;
		else if(s_PidItemZ.intergral <= -100) 
			s_PidItemZ.intergral = -100;
		s_PidItemZ.differential =  s_PidItemZ.difference  - s_PidItemZ.tempDiffer;
	  s_PidItemZ.tempDiffer = s_PidItemZ.difference;
		s_PidOut[2] = s_PidZ.p*s_PidItemZ.difference + s_PidZ.d*s_PidItemZ.differential + s_PidZ.i*s_PidItemZ.intergral;

		/*Yaw方向的pid控制*/
		s_PidItemYaw.difference =  expectYaw - currentYaw;
		s_PidItemYaw.intergral += s_PidItemYaw.difference;
		if(s_PidItemYaw.intergral >= 100)		
			s_PidItemYaw.intergral = 100;
		else if(s_PidItemYaw.intergral <= -100) 
			s_PidItemYaw.intergral = -100;
		s_PidItemYaw.differential =  s_PidItemYaw.difference  - s_PidItemYaw.tempDiffer;
	  s_PidItemYaw.tempDiffer = s_PidItemYaw.difference;
		s_PidOut[3] = s_PidYaw.p*s_PidItemYaw.difference + s_PidYaw.d*s_PidItemYaw.differential + s_PidYaw.i*s_PidItemYaw.intergral;
		return s_PidOut;
	}
};

	  void APMLanding::send_body_velxyz_setpoint(const Eigen::Vector3d& vel_sp, float yaw_sp)
	{
	    mavros_msgs::PositionTarget pos_setpoint;
	    //Bitmask toindicate which dimensions should be ignored (1 means ignore,0 means not ignore; Bit 10 must set to 0)
	    //Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7:ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11:yaw, bit 12:yaw_rate
	    //Bit 10 should set to 0, means is not force sp
	    pos_setpoint.type_mask = 1 + 2 + 4 +/* 8 + 16 + 32 +*/ 64 + 128 + 256 + 512 + 1024/* + 2048*/;
	    pos_setpoint.coordinate_frame = 8;

	    pos_setpoint.velocity.x = vel_sp[0];
	    pos_setpoint.velocity.y = vel_sp[1];
	    //pos_setpoint.velocity.z = vel_sp[2];
	    pos_setpoint.velocity.z = 0;
	    pos_setpoint.yaw_rate = yaw_sp;
	    mavros_setpoint_pos_pub_.publish(pos_setpoint);
	}
	//  void send_body_posxyz_setpoint(const Eigen::Vector3d& vel_sp, float yaw_sp)
	// {
	//     mavros_msgs::PositionTarget pos_setpoint;
	//     //Bitmask toindicate which dimensions should be ignored (1 means ignore,0 means not ignore; Bit 10 must set to 0)
	//     //Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7:ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11:yaw, bit 12:yaw_rate
	//     //Bit 10 should set to 0, means is not force sp
	//     pos_setpoint.type_mask = /*1 + 2 + 4 */+ 8 + 16 + 32 + 64 + 128 + 256 + 512 + 1024/* + 2048*/;
	//     pos_setpoint.coordinate_frame = 8;

	//     pos_setpoint.position.x = vel_sp[0];
	//     pos_setpoint.position.y = vel_sp[1];
	//     //pos_setpoint.velocity.z = vel_sp[2];
	//     pos_setpoint.position.z = 0;
	//     pos_setpoint.yaw_rate = yaw_sp;
	//     mavros_setpoint_pos_pub_.publish(pos_setpoint);
	// }
bool avoid=false;
int avoidnum=0;
int avoidnumold=0;
int avoidnumnew=0;
double distance2b=0;
float Beta=0;
bool enablelocalmove=false;
float get_current_heading();
geometry_msgs::Point get_current_location();
void set_destination(double x, double y, double z, double psi);
void local_move(float x, float y, float z, float psi);

//get armed state
void state_cb(const mavros_msgs::State::ConstPtr& msg)
{
  current_state_g = *msg;
}
geometry_msgs::Point enu_2_local(nav_msgs::Odometry current_pose_enu)
{
  float x = current_pose_enu.pose.pose.position.x;
  float y = current_pose_enu.pose.pose.position.y;
  float z = current_pose_enu.pose.pose.position.z;
  float deg2rad = (M_PI/180);
  geometry_msgs::Point current_pos_local;
  current_pos_local.x = x*cos((local_offset_g - 90)*deg2rad) - y*sin((local_offset_g - 90)*deg2rad);
  current_pos_local.y = x*sin((local_offset_g - 90)*deg2rad) + y*cos((local_offset_g - 90)*deg2rad);
  current_pos_local.z = z;

  return current_pos_local;

  //ROS_INFO("Local position %f %f %f",X, Y, Z);
}
//get current position of drone
  geometry_msgs::Point gps_to_enu(float latitude, float longitude, float altitude) {
    // 这里假设ENU坐标系的原点是GPS起始点，实际情况可能需要根据具体需求进行调整

    // 计算ENU坐标系下的XYZ坐标
    float x = (longitude - REFERENCE_LONGITUDE) * 111319.9; // 经度变化大约每111319.9米变化1度
    float y = (latitude - REFERENCE_LATITUDE) * 111319.9; // 纬度变化大约每111319.9米变化1度
    float z = altitude; // 高度保持不变

    geometry_msgs::Point enu_point;
    enu_point.x = x;
    enu_point.y = y;
    enu_point.z = z;

    return enu_point;
}
void pose_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
  current_pose_g = *msg;
  enu_2_local(current_pose_g);
  float q0 = current_pose_g.pose.pose.orientation.w;
  float q1 = current_pose_g.pose.pose.orientation.x;
  float q2 = current_pose_g.pose.pose.orientation.y;
  float q3 = current_pose_g.pose.pose.orientation.z;
  float psi = atan2((2*(q0*q3 + q1*q2)), (1 - 2*(pow(q2,2) + pow(q3,2))) );
  //ROS_INFO("Current Heading %f ENU", psi*(180/M_PI));
  //Heading is in ENU
  //IS YAWING COUNTERCLOCKWISE POSITIVE?
  current_heading_g = psi*(180/M_PI) - local_offset_g;
  //ROS_INFO("Current Heading %f origin", current_heading_g);
  //ROS_INFO("x: %f y: %f z: %f", current_pose_g.pose.pose.position.x, current_pose_g.pose.pose.position.y, current_pose_g.pose.pose.position.z);
}

void Initialize()
{
  desire_pose_x=0;
  desire_pose_y=0;
  desire_pose_z=5;
  desire_pose_[0] = desire_pose_x;
  desire_pose_[1] = desire_pose_y;
  desire_pose_[2] = desire_pose_z;
  desire_yawVel_ = 0;
  desire_xyVel_[0]  = 0;
  desire_xyVel_[1]  = 0;
  desire_xyVel_[2]  = 0;
  s_PidXY.p=20;
  //s_PidXY.d=0.05;
  s_PidXY.d=0.05;
 // s_PidXY.i=0.001;
  s_PidXY.i=0.01;
  s_PidZ.p=0.2;
  detect_state=false;
}

void gpsCallback(const sensor_msgs::NavSatFix::ConstPtr& msg)
{
   GPSCoordinate = *msg;
   //ROS_INFO("GPSCoordinate.latitude=%f,GPSCoordinate.longitude=%f",GPSCoordinate.latitude,GPSCoordinates.longitude);
}
double get_current_lon()
{
	return GPSCoordinate.longitude;
}
double get_current_lat()
{
	return GPSCoordinate.latitude;
}
double getalt()
{

	return GPSCoordinate.altitude;
}
geometry_msgs::Point get_current_location()
{
	geometry_msgs::Point current_pos_local;
	current_pos_local = enu_2_local(current_pose_g);
	return current_pos_local;

}
float get_current_heading()
{
	return current_heading_g;
}

double calculateDistance(double lat1, double lon1, double lat2, double lon2) {
    const double RADIUS_EARTH = 6371000.0; // 地球平均半径，单位为米
    double dLat = degreesToRadians(lat2 - lat1);
    double dLon = degreesToRadians(lon2 - lon1);

    double a = sin(dLat / 2) * sin(dLat / 2) +
               cos(degreesToRadians(lat1)) * cos(degreesToRadians(lat2)) *
               sin(dLon / 2) * sin(dLon / 2);
    double c = 2 * atan2(sqrt(a), sqrt(1 - a));
    double distance = RADIUS_EARTH * c;

    return distance;
}
//set orientation of the drone (drone should always be level) 
// Heading input should match the ENU coordinate system
/**
\ingroup control_functions
This function is used to specify the drone’s heading in the local reference frame. Psi is a counter clockwise rotation following the drone’s reference frame defined by the x axis through the right side of the drone with the y axis through the front of the drone. 
@returns n/a
*/
void set_heading(float heading)
{
  local_desired_heading_g = heading; 
  heading = heading + correction_heading_g + local_offset_g;
  
  ROS_INFO("Desired Heading %f ", local_desired_heading_g);
  float yaw = heading*(M_PI/180);
  float pitch = 0;
  float roll = 0;

  float cy = cos(yaw * 0.5);
  float sy = sin(yaw * 0.5);
  float cr = cos(roll * 0.5);
  float sr = sin(roll * 0.5);
  float cp = cos(pitch * 0.5);
  float sp = sin(pitch * 0.5);

  float qw = cy * cr * cp + sy * sr * sp;
  float qx = cy * sr * cp - sy * cr * sp;
  float qy = cy * cr * sp + sy * sr * cp;
  float qz = sy * cr * cp - cy * sr * sp;

  waypoint_g.pose.orientation.w = qw;
  waypoint_g.pose.orientation.x = qx;
  waypoint_g.pose.orientation.y = qy;
  waypoint_g.pose.orientation.z = qz;
  local_pos_pub.publish(waypoint_g);
}
// set position to fly to in the local frame
/**
\ingroup control_functions
This function is used to command the drone to fly to a waypoint. These waypoints should be specified in the local reference frame. This is typically defined from the location the drone is launched. Psi is counter clockwise rotation following the drone’s reference frame defined by the x axis through the right side of the drone with the y axis through the front of the drone. 
@returns n/a
*/
void set_destination(double x, double y, double z, double psi)
{
	set_heading(psi);
	//transform map to local
	double deg2rad = (M_PI/180);
	double Xlocal = x*cos((correction_heading_g + local_offset_g - 90)*deg2rad) - y*sin((correction_heading_g + local_offset_g - 90)*deg2rad);
	double Ylocal = x*sin((correction_heading_g + local_offset_g - 90)*deg2rad) + y*cos((correction_heading_g + local_offset_g - 90)*deg2rad);
	double Zlocal = z;

	x = Xlocal + correction_vector_g.position.x + local_offset_pose_g.x;
	y = Ylocal + correction_vector_g.position.y + local_offset_pose_g.y;
	z = Zlocal + correction_vector_g.position.z + local_offset_pose_g.z;
	ROS_INFO("Destination set to x: %f y: %f z: %f origin frame", x, y, z);
  //ROS_INFO("Heading=%f",current_heading_g);
	waypoint_g.pose.position.x = x;
	waypoint_g.pose.position.y = y;
	waypoint_g.pose.position.z = z;

	local_pos_pub.publish(waypoint_g);
	
}

void local_move(float x, float y, float z, float psi)
{
	set_heading(psi);
	float deg2rad = (M_PI/180);
	float Xlocal = x*cos((correction_heading_g + local_offset_g - 90)*deg2rad) - y*sin((correction_heading_g + local_offset_g - 90)*deg2rad);
	float Ylocal = x*sin((correction_heading_g + local_offset_g - 90)*deg2rad) + y*cos((correction_heading_g + local_offset_g - 90)*deg2rad);
	float Zlocal = z;

  waypoint_g.pose.position.x = Xlocal;
	waypoint_g.pose.position.y = Ylocal;
	waypoint_g.pose.position.z = Zlocal;
	ROS_INFO("local Destination set to x: %f y: %f z: %f origin frame", x, y, z);
	local_pos_pub.publish(waypoint_g);
}

void set_destination_lla(float lat, float lon, float alt, float heading)
{
	geographic_msgs::GeoPoseStamped lla_msg;
	// mavros_msgs::GlobalPositionTarget 
	lla_msg.pose.position.latitude = lat;
	lla_msg.pose.position.longitude = lon;
	lla_msg.pose.position.altitude = alt;
	float yaw = heading*(M_PI/180);
	float pitch = 0;
	float roll = 0;

	float cy = cos(yaw * 0.5);
	float sy = sin(yaw * 0.5);
	float cr = cos(roll * 0.5);
	float sr = sin(roll * 0.5);
	float cp = cos(pitch * 0.5);
	float sp = sin(pitch * 0.5);

	float qw = cy * cr * cp + sy * sr * sp;
	float qx = cy * sr * cp - sy * cr * sp;
	float qy = cy * cr * sp + sy * sr * cp;
	float qz = sy * cr * cp - cy * sr * sp;

	lla_msg.pose.orientation.w = qw;
	lla_msg.pose.orientation.x = qx;
	lla_msg.pose.orientation.y = qy;
	lla_msg.pose.orientation.z = qz;
	global_lla_pos_pub.publish(lla_msg);
	ROS_INFO("set_destination_lla lat=%f,lon=%f",lat,lon);
}
void set_destination_lla_raw(float lat, float lon, float alt, float heading)
{
	mavros_msgs::GlobalPositionTarget lla_msg;
	lla_msg.coordinate_frame = lla_msg.FRAME_GLOBAL_TERRAIN_ALT;
	lla_msg.type_mask = lla_msg.IGNORE_VX | lla_msg.IGNORE_VY | lla_msg.IGNORE_VZ | lla_msg.IGNORE_AFX | lla_msg.IGNORE_AFY | lla_msg.IGNORE_AFZ | lla_msg.IGNORE_YAW | lla_msg.IGNORE_YAW_RATE ; 
	lla_msg.latitude = lat;
	lla_msg.longitude = lon;
	lla_msg.altitude = alt;
	// lla_msg.yaw = heading;
	global_lla_pos_pub_raw.publish(lla_msg);
	ROS_INFO("set Destination to %f ,%f",lat,lon);
}


/**
\ingroup control_functions
Wait for connect is a function that will hold the program until communication with the FCU is established.
@returns 0 - connected to fcu 
@returns -1 - failed to connect to drone
*/
int wait4connect()
{
	ROS_INFO("Waiting for FCU connection");
	// wait for FCU connection
	while (ros::ok() && !current_state_g.connected)
	{
		ros::spinOnce();
		ros::Duration(0.01).sleep();
	}
	if(current_state_g.connected)
	{
		ROS_INFO("Connected to FCU");	
		return 0;
	}else{
		ROS_INFO("Error connecting to drone");
		return -1;	
	}
	
	
}
/**
\ingroup control_functions
Wait for strat will hold the program until the user signals the FCU to enther mode guided. This is typically done from a switch on the safety pilot’s remote or from the ground control station.
@returns 0 - mission started
@returns -1 - failed to start mission
*/
int wait4start()
{
	ROS_INFO("Waiting for user to set mode to GUIDED");
	while(ros::ok() && current_state_g.mode != "GUIDED")
	{
	    ros::spinOnce();
	    ros::Duration(0.01).sleep();
  	}
  	if(current_state_g.mode == "GUIDED")
	{
		ROS_INFO("Mode set to GUIDED. Mission starting");
		return 0;
	}else{
		ROS_INFO("Error starting mission!!");
		return -1;	
	}
}
/**
\ingroup control_functions
This function will create a local reference frame based on the starting location of the drone. This is typically done right before takeoff. This reference frame is what all of the the set destination commands will be in reference to.
@returns 0 - frame initialized
*/
int initialize_local_frame()
{
	//set the orientation of the local reference frame
	ROS_INFO("Initializing local coordinate system");
	local_offset_g = 0;
	for (int i = 1; i <= 30; i++) {
		ros::spinOnce();
		ros::Duration(0.1).sleep();

		

		float q0 = current_pose_g.pose.pose.orientation.w;
		float q1 = current_pose_g.pose.pose.orientation.x;
		float q2 = current_pose_g.pose.pose.orientation.y;
		float q3 = current_pose_g.pose.pose.orientation.z;
		float psi = atan2((2*(q0*q3 + q1*q2)), (1 - 2*(pow(q2,2) + pow(q3,2))) ); // yaw

		local_offset_g += psi*(180/M_PI);

		local_offset_pose_g.x = local_offset_pose_g.x + current_pose_g.pose.pose.position.x;
		local_offset_pose_g.y = local_offset_pose_g.y + current_pose_g.pose.pose.position.y;
		local_offset_pose_g.z = local_offset_pose_g.z + current_pose_g.pose.pose.position.z;
		// ROS_INFO("current heading%d: %f", i, local_offset_g/i);
	}
	local_offset_pose_g.x = local_offset_pose_g.x/30;
	local_offset_pose_g.y = local_offset_pose_g.y/30;
	local_offset_pose_g.z = local_offset_pose_g.z/30;
	local_offset_g /= 30;
	ROS_INFO("Coordinate offset set");
	ROS_INFO("the X' axis is facing: %f", local_offset_g);
	return 0;
}

int arm()
{
	//intitialize first waypoint of mission
	set_destination(0,0,0,0);
	for(int i=0; i<100; i++)
	{
		local_pos_pub.publish(waypoint_g);
		ros::spinOnce();
		ros::Duration(0.01).sleep();
	}
	// arming
	ROS_INFO("Arming drone");
	mavros_msgs::CommandBool arm_request;
	arm_request.request.value = true;
	while (!current_state_g.armed && !arm_request.response.success && ros::ok())
	{
		ros::Duration(.1).sleep();
		arming_client.call(arm_request);
		local_pos_pub.publish(waypoint_g);
	}
	if(arm_request.response.success)
	{
		ROS_INFO("Arming Successful");	
		return 0;
	}else{
		ROS_INFO("Arming failed with %d", arm_request.response.success);
		return -1;	
	}
}

/**
\ingroup control_functions
The takeoff function will arm the drone and put the drone in a hover above the initial position. 
@returns 0 - nominal takeoff 
@returns -1 - failed to arm 
@returns -2 - failed to takeoff
*/
int takeoff(float takeoff_alt)
{
	//intitialize first waypoint of mission
	set_destination(0,0,takeoff_alt,0);
	for(int i=0; i<100; i++)
	{
		local_pos_pub.publish(waypoint_g);
		ros::spinOnce();
		ros::Duration(0.01).sleep();
	}
	// arming
	ROS_INFO("Arming drone");
	mavros_msgs::CommandBool arm_request;
	arm_request.request.value = true;
	while (!current_state_g.armed && !arm_request.response.success && ros::ok())
	{
		ros::Duration(.1).sleep();
		arming_client.call(arm_request);
		local_pos_pub.publish(waypoint_g);
	}
	if(arm_request.response.success)
	{
		ROS_INFO("Arming Successful");	
	}else{
		ROS_INFO("Arming failed with %d", arm_request.response.success);
		return -1;	
	}

	//request takeoff
	
	mavros_msgs::CommandTOL srv_takeoff;
	srv_takeoff.request.altitude = takeoff_alt;
	if(takeoff_client.call(srv_takeoff)){
		sleep(3);
		ROS_INFO("takeoff sent %d", srv_takeoff.response.success);
	}else{
		ROS_ERROR("Failed Takeoff");
		return -2;
	}
	sleep(2);
	return 0; 
}
/**
\ingroup control_functions
This function returns an int of 1 or 0. THis function can be used to check when to request the next waypoint in the mission. 
@return 1 - waypoint reached 
@return 0 - waypoint not reached
*/
int check_waypoint_reached(float pos_tolerance=0.3, float heading_tolerance=0.01)
{
	local_pos_pub.publish(waypoint_g);
	
	//check for correct position 
	float deltaX = abs(waypoint_g.pose.position.x - current_pose_g.pose.pose.position.x);
    float deltaY = abs(waypoint_g.pose.position.y - current_pose_g.pose.pose.position.y);
    float deltaZ = 0; //abs(waypoint_g.pose.position.z - current_pose_g.pose.pose.position.z);
    float dMag = sqrt( pow(deltaX, 2) + pow(deltaY, 2) + pow(deltaZ, 2) );
    // ROS_INFO("dMag %f", dMag);
    // ROS_INFO("current pose x %F y %f z %f", (current_pose_g.pose.pose.position.x), (current_pose_g.pose.pose.position.y), (current_pose_g.pose.pose.position.z));
    // ROS_INFO("waypoint pose x %F y %f z %f", waypoint_g.pose.position.x, waypoint_g.pose.position.y,waypoint_g.pose.position.z);
    //check orientation
    float cosErr = cos(current_heading_g*(M_PI/180)) - cos(local_desired_heading_g*(M_PI/180));
    float sinErr = sin(current_heading_g*(M_PI/180)) - sin(local_desired_heading_g*(M_PI/180));
    
    float headingErr = sqrt( pow(cosErr, 2) + pow(sinErr, 2) );

    // ROS_INFO("current heading %f", current_heading_g);
    // ROS_INFO("local_desired_heading_g %f", local_desired_heading_g);
    // ROS_INFO("current heading error %f", headingErr);

    if( dMag < pos_tolerance && headingErr < heading_tolerance)
	{
		return 1;
	}else{
		return 0;
	}
}


/**
\ingroup control_functions
this function changes the mode of the drone to a user specified mode. This takes the mode as a string. ex. set_mode("GUIDED")
@returns 1 - mode change successful
@returns 0 - mode change not successful
*/
int set_mode(std::string mode)
{
	mavros_msgs::SetMode srv_setMode;
    srv_setMode.request.base_mode = 0;
    srv_setMode.request.custom_mode = mode.c_str();
    if(set_mode_client.call(srv_setMode)){
      ROS_INFO("setmode send ok");
	  return 0;
    }else{
      ROS_ERROR("Failed SetMode");
      return -1;
    }
}

/**
\ingroup control_functions
this function changes the mode of the drone to land
@returns 1 - mode change successful
@returns 0 - mode change not successful
*/
int land()
{
  mavros_msgs::CommandTOL srv_land;
  if(land_client.call(srv_land) && srv_land.response.success)
  {
    ROS_INFO("land sent %d", srv_land.response.success);
    return 0;
  }else{
    ROS_ERROR("Landing failed");
    return -1;
  }
}
/**
\ingroup control_functions
This function is used to change the speed of the vehicle in guided mode. it takes the speed in meters per second as a float as the input
@returns 0 for success
*/

int auto_set_current_waypoint(int seq)
{
	mavros_msgs::WaypointSetCurrent wp_set_cur_msg;
	wp_set_cur_msg.request.wp_seq = seq;
	ROS_INFO("setting current wp to wp # %d", seq);
	if(auto_waypoint_set_current_client.call(wp_set_cur_msg))
	{
		ROS_INFO("set current wp secceeded %d", wp_set_cur_msg.response.success);
	}else{
		ROS_ERROR("set current wp failed %d", wp_set_cur_msg.response.success);
	}
	return 0;
}

/**
\ingroup control_functions
used to set yaw when running lla waypoint missions
param1: Angle				target angle, 0 is north																			deg
param2: Angular Speed		angular speed																						deg/s
param3: Direction			direction: -1: counter clockwise, 1: clockwise					min: -1 max:1 increment:2	
param4: Relative			0: absolute angle, 1: relative offset							min:0 max:1 increment:1	
@returns 0 for success
*/

int takeoff_global(float lat, float lon, float alt)
{
    mavros_msgs::CommandTOL srv_takeoff_global;
    srv_takeoff_global.request.min_pitch = 0;
    //srv_takeoff_global.request.yaw = heading; //check yaw angle
    srv_takeoff_global.request.latitude = lat;
    srv_takeoff_global.request.longitude = lon;
    srv_takeoff_global.request.altitude = alt;
        
    if(takeoff_client.call(srv_takeoff_global)){
        sleep(3);
        ROS_INFO("takeoff sent at the provided GPS coordinates %d", srv_takeoff_global.response.success);
    }
    else
    {
        ROS_ERROR("Failed Takeoff %d", srv_takeoff_global.response.success);
        
    }
    sleep(2);
    return 0;
}
/**
\ingroup control_functions
This function is called at the beginning of a program and will start of the communication links to the FCU. The function requires the program's ros nodehandle as an input 
@returns n/a
*/
int init_publisher_subscriber(ros::NodeHandle controlnode)
{
	std::string ros_namespace;
	if (!controlnode.hasParam("namespace"))
	{

		ROS_INFO("using default namespace");
	}else{
		controlnode.getParam("namespace", ros_namespace);
		ROS_INFO("using namespace %s", ros_namespace.c_str());
	}
	local_pos_pub = controlnode.advertise<geometry_msgs::PoseStamped>((ros_namespace + "/mavros/setpoint_position/local").c_str(), 10);
	global_lla_pos_pub = controlnode.advertise<geographic_msgs::GeoPoseStamped>((ros_namespace + "/mavros/setpoint_position/global").c_str(), 10);
	global_lla_pos_pub_raw = controlnode.advertise<mavros_msgs::GlobalPositionTarget>((ros_namespace + "/mavros/setpoint_raw/global").c_str(), 10);
	currentPos = controlnode.subscribe<nav_msgs::Odometry>((ros_namespace + "/mavros/global_position/local").c_str(), 10, pose_cb);
	state_sub = controlnode.subscribe<mavros_msgs::State>((ros_namespace + "/mavros/state").c_str(), 10, state_cb);
	arming_client = controlnode.serviceClient<mavros_msgs::CommandBool>((ros_namespace + "/mavros/cmd/arming").c_str());
	land_client = controlnode.serviceClient<mavros_msgs::CommandTOL>((ros_namespace + "/mavros/cmd/land").c_str());
	set_mode_client = controlnode.serviceClient<mavros_msgs::SetMode>((ros_namespace + "/mavros/set_mode").c_str());
	takeoff_client = controlnode.serviceClient<mavros_msgs::CommandTOL>((ros_namespace + "/mavros/cmd/takeoff").c_str());
	command_client = controlnode.serviceClient<mavros_msgs::CommandLong>((ros_namespace + "/mavros/cmd/command").c_str());
	auto_waypoint_pull_client = controlnode.serviceClient<mavros_msgs::WaypointPull>((ros_namespace + "/mavros/mission/pull").c_str());
	auto_waypoint_push_client = controlnode.serviceClient<mavros_msgs::WaypointPush>((ros_namespace + "/mavros/mission/push").c_str());
	auto_waypoint_set_current_client = controlnode.serviceClient<mavros_msgs::WaypointSetCurrent>((ros_namespace + "/mavros/mission/set_current").c_str());
	gps_sub = controlnode.subscribe<sensor_msgs::NavSatFix>((ros_namespace+"/mavros/global_position/global").c_str(), 10, gpsCallback);
	
	return 0;
}